﻿#include "LECalibrator.h"
#include <QtAlgorithms>
#include <QDebug>
#include <gsl/gsl_sort_double.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_nan.h>

LECalibrator::LECalibrator()
    : m_xa(nullptr),
      m_ya(nullptr),
      m_size(0),
      m_interp(nullptr),
      m_interp_rev(nullptr),
      m_accel(nullptr),
      m_accel_rev(nullptr)
{

}

LECalibrator::~LECalibrator()
{
    reset();
}

void LECalibrator::print()
{
    for(int i = 0; i < m_size; i ++)
    {
        qDebug() << "(" << m_xa[i] << ", " << m_ya[i] << ")";
    }
}

double LECalibrator::encoderPosToTruePos(double value)
{
    if(m_size == 0 || value <= m_xa[0])
    {
        return value;
    }
    if(value >= m_xa[m_size - 1]) //如果超出最后一个点，则外插
    {
        return m_ya[m_size - 1] + (value - m_xa[m_size - 1]);
    }

    double ret = 0;
    if(m_interp)
    {
        ret = gsl_interp_eval (m_interp, m_xa, m_ya, value, m_accel);

        if(ret == GSL_NAN)
        {
            return value;
        }
        return ret;
    }
    return value;
}

double LECalibrator::truePosToEncoderPos(double value)
{
    if(m_size == 0 || value <= m_ya[0])
    {
        return value;
    }
    if(value >= m_ya[m_size - 1]) //如果超出最后一个点，则外插
    {
        return m_xa[m_size - 1] + (value - m_ya[m_size - 1]);
    }

    double ret = 0;
    if(m_interp_rev)
    {
        ret = gsl_interp_eval (m_interp_rev, m_ya, m_xa, value, m_accel_rev);

        if(ret == GSL_NAN)
        {
            return value;
        }
        return ret;
    }
    return value;
}


void LECalibrator::reset()
{
    if(m_xa)
    {
        delete[] m_xa;
        delete[] m_ya;
        m_xa = nullptr;
        m_ya = nullptr;
    }
    m_size = 0;
    if(m_interp)
    {
        gsl_interp_free(m_interp);
        gsl_interp_free(m_interp_rev);
        gsl_interp_accel_free(m_accel);
        gsl_interp_accel_free(m_accel_rev);
        m_interp = nullptr;
        m_accel = nullptr;
        m_accel_rev = nullptr;
    }
}

inline double round( double x)
{
    int ret = static_cast<int>(x + 0.5);
    return ret;
}

void LECalibrator::build()
{
    gsl_sort(m_xa, 1, m_size);

    m_ya[0] = m_xa[0];
    for(int i = 1; i < m_size; i++)
    {
        m_ya[i] = round( m_xa[i] - m_xa[0] ) + m_xa[0];
    }
    m_interp = gsl_interp_alloc(gsl_interp_linear, m_size);
    m_interp_rev = gsl_interp_alloc(gsl_interp_linear, m_size);
    gsl_interp_init(m_interp, m_xa, m_ya, m_size);
    gsl_interp_init(m_interp_rev, m_ya, m_xa, m_size);
    m_accel = gsl_interp_accel_alloc();
    m_accel_rev = gsl_interp_accel_alloc();
}

void LECalibrator::loadPoints(double p[], int N)
{
    if(N < 2)
    {
        return;
    }

    reset();
    m_size = N;
    m_xa = new double[m_size];
    m_ya = new double[m_size];

    for(int i = 0; i < m_size; i++)
    {
        m_xa[i] = p[i];
    }

    build();
}

void LECalibrator::loadPoints(QVector<double> points)
{
    if(points.size() < 2)
    {
        return;
    }

    reset();
    m_size = points.size();
    m_xa = new double[m_size];
    m_ya = new double[m_size];

    for(int i = 0; i < m_size; i++)
    {
        m_xa[i] = points.at(i);
    }
    gsl_sort(m_xa, 1, m_size);

    build();
}
